// 1.包含头文件
#include "rclcpp/rclcpp.hpp"
#include <std_msgs/msg/float64_multi_array.hpp>
#include "std_msgs/msg/float32.hpp"
#include <iostream>
#include "canconfigimu.h"

using namespace std::chrono_literals;
// 3.自定义节点类；
class Can_Driver : public rclcpp::Node
{
public:
  Can_Driver() : Node("can_driver_node_cpp")
  {

    // 创建速度指令订阅者回调
    this->twist_sub_ = this->create_subscription<geometry_msgs::msg::Twist>("cmd_vel", 10, std::bind(&Can_Driver::twistCallback, this, std::placeholders::_1));
    // // 创建控制线程
    // std::thread{std::bind(&Can_Driver::control_thread, this)}.detach();
    this->speed_sub_ = this->create_subscription<std_msgs::msg::Int32>("speed_true", 10, std::bind(&Can_Driver::speedCallback, this, std::placeholders::_1));
    // imu数据订阅
    // this->imu_data_subr = this->create_subscription<std_msgs::msg::Float64MultiArray>("imu_data_pub", 10, std::bind(&Can_Driver::imu_data, this, std::placeholders::_1));
    // flag订阅
    // this->flag_sub_ = this->create_subscription<std_msgs::msg::Int32>("flag", 2, std::bind(&Can_Driver::flag_cb, this, std::placeholders::_1));
    // // 创建实时速度发布方
    // rtspeed1_pub_ = this->create_publisher<std_msgs::msg::Int32>("rt1_speed", 10);
    // rtspeed2_pub_ = this->create_publisher<std_msgs::msg::Int32>("rt2_speed", 10);
    // rtspeed3_pub_ = this->create_publisher<std_msgs::msg::Int32>("rt3_speed", 10);
    // rtspeed4_pub_ = this->create_publisher<std_msgs::msg::Int32>("rt4_speed", 10);
    // 定时器
    // sp_timer_ = this->create_wall_timer(0.01s, std::bind(&Can_Driver::on_rtsp_timer, this));
    // 创建位置指令订阅回调
    this->pos_sub_ = this->create_subscription<std_msgs::msg::Float32>("pos_true", 10, std::bind(&Can_Driver::posCallback, this, std::placeholders::_1));
    this->vel_sub_ = this->create_subscription<std_msgs::msg::Float32>("vel_true", 10, std::bind(&Can_Driver::velCallback, this, std::placeholders::_1));
  }

private:
  int flag = 0;
  // 键盘控制节点订阅
  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr twist_sub_;

  // 速度发布节点订阅
  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr speed_sub_;

  // 位置节点订阅
  rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr pos_sub_;
  rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr vel_sub_;

  // flag订阅
  // rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr flag_sub_;

  // // 四个车轮的实时速度反馈
  // rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr rtspeed1_pub_;
  // rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr rtspeed2_pub_;
  // rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr rtspeed3_pub_;
  // rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr rtspeed4_pub_;

  // 订阅R角度
  // rclcpp::Subscription<std_msgs::msg::Float64MultiArray>::SharedPtr imu_data_subr;

  // 定义计时器
  rclcpp::TimerBase::SharedPtr sp_timer_;

  void speedCallback(const std_msgs::msg::Int32::SharedPtr speed_msg)
  {
    V_x = speed_msg->data;
    RCLCPP_INFO(this->get_logger(), "%d", V_x);
  }

  // void imu_data(const std_msgs::msg::Float64MultiArray::SharedPtr imu_data)
  // {
  //   // std::this_thread::sleep_for(std::chrono::milliseconds(100));加上这个车辆会卡
  //   pitch = imu_data->data[1] + 90;
  //   roll = imu_data->data[0];
  //   // std::cout<<"out:" << pitch << std::endl;
  // }
  // void flag_cb(const std_msgs::msg::Int32::SharedPtr flag_msg)
  // {
  //   flag = flag_msg->data;
  // }
  // 位置订阅回调
  void velCallback(const std_msgs::msg::Float32::SharedPtr vel_msg)
  {
    // std::cout << "shuchu" << std::endl;
    int V_x_1_3 = round(vel_msg->data);
    std::ostringstream stream_1_3;
    stream_1_3 << std::hex << V_x_1_3;
    std::string hexStr_1_3 = stream_1_3.str();
    unsigned int num_1 = std::stoul(hexStr_1_3, nullptr, 16);
    output[0] = num_1;
    BYTE speed_data_05[5] = {
        0x1d,
        output[0] & 0xFF,         // 获取最低字节
        (output[0] >> 8) & 0xFF,  // 获取次低字节
        (output[0] >> 16) & 0xFF, // 获取次高字节
        (output[0] >> 24) & 0xFF  // 获取最高字节
    };
    // 发送数据
    SendData(send_motor_speed, motor_id_05, speed_data_05, 5);
  }
  void posCallback(const std_msgs::msg::Float32::SharedPtr pos_msg)
  {
    // 速度控制 500
    BYTE pos_p[5] = {0x24, 0xc8, 0x00, 0x00, 0x00};
    SendData(send_motor_speed, motor_id_05, pos_p, 5);
    // 速度控制 -500
    BYTE pos_r[5] = {0x25, 0x38, 0xff, 0xff, 0xff};
    SendData(send_motor_speed, motor_id_05, pos_r, 5);
    // 取负值，保证向右转为正数
    float pos_msg_ = -pos_msg->data;
    int motor_val = pos_val(pos_msg_);
    // 数据转换，机械臂电机
    std::cout << "十进制数: " << motor_val << std::endl;
    std::ostringstream stream_5;
    stream_5 << std::hex << motor_val;
    std::string hexStr_5 = stream_5.str();
    std::cout << "十六进制数: " << hexStr_5 << std::endl;
    // 机械臂位置
    unsigned int num_1 = std::stoul(hexStr_5, nullptr, 16);
    output[0] = num_1;
    BYTE speed_data_05[5] = {
        0x1e,
        output[0] & 0xFF,         // 获取最低字节
        (output[0] >> 8) & 0xFF,  // 获取次低字节
        (output[0] >> 16) & 0xFF, // 获取次高字节
        (output[0] >> 24) & 0xFF  // 获取最高字节
    };
    SendData(send_motor_speed, motor_id_05, speed_data_05, 5);
  }

  void twistCallback(const geometry_msgs::msg::Twist::SharedPtr msg)
  {
    float Straight_f = msg->linear.x;
    float Turn_f = msg->angular.z;
    if (V_x > 2000)
    {
      // 限制最大速度
      V_x = 2000;
    }

    if (flag == 0)
    {
      // 前进
      if (Straight_f > 0)
      {
        // 数据转换（1&3轮）
        // std::cout << "十进制数: " << V_x << std::endl;
        int V_x_1_3 = round(V_x);
        // std::cout << "十进制数00: " << V_x_1_3 << std::endl;
        std::ostringstream stream_1_3;
        stream_1_3 << std::hex << V_x_1_3;
        std::string hexStr_1_3 = stream_1_3.str();
        // std::cout << "十六进制数: " << hexStr_1_3 << std::endl;
        // 左前轮速度(1)
        unsigned int num_1 = std::stoul(hexStr_1_3, nullptr, 16);
        output[0] = num_1;
        BYTE speed_data_01[5] = {
            0x1d,
            output[0] & 0xFF,         // 获取最低字节
            (output[0] >> 8) & 0xFF,  // 获取次低字节
            (output[0] >> 16) & 0xFF, // 获取次高字节
            (output[0] >> 24) & 0xFF  // 获取最高字节
        };
        // 左后轮速度(3)
        unsigned int num_3 = std::stoul(hexStr_1_3, nullptr, 16);
        output[2] = num_3;
        BYTE speed_data_03[5] = {
            0x1d,
            output[2] & 0xFF,         // 获取最低字节
            (output[2] >> 8) & 0xFF,  // 获取次低字节
            (output[2] >> 16) & 0xFF, // 获取次高字节
            (output[2] >> 24) & 0xFF  // 获取最高字节
        };
        // 数据转换（2&4轮）
        int V_x_2_4 = -round(V_x);
        std::ostringstream stream_2_4;
        stream_2_4 << std::hex << V_x_2_4;
        std::string hexStr_2_4 = stream_2_4.str();
        // 右前轮速度(2)
        unsigned int num_2 = std::stoul(hexStr_2_4, nullptr, 16);
        output[1] = num_2;
        BYTE speed_data_02[5] = {
            0x1d,
            output[1] & 0xFF,         // 获取最低字节
            (output[1] >> 8) & 0xFF,  // 获取次低字节
            (output[1] >> 16) & 0xFF, // 获取次高字节
            (output[1] >> 24) & 0xFF  // 获取最高字节
        };
        // 右前轮速度(4)
        unsigned int num_4 = std::stoul(hexStr_2_4, nullptr, 16);
        output[1] = num_4;
        BYTE speed_data_04[5] = {
            0x1d,
            output[1] & 0xFF,         // 获取最低字节
            (output[1] >> 8) & 0xFF,  // 获取次低字节
            (output[1] >> 16) & 0xFF, // 获取次高字节
            (output[1] >> 24) & 0xFF  // 获取最高字节
        };
        // 发送数据
        SendData(send_motor_speed, motor_id_01, speed_data_01, 5);
        SendData(send_motor_speed, motor_id_02, speed_data_02, 5);
        SendData(send_motor_speed, motor_id_03, speed_data_03, 5);
        SendData(send_motor_speed, motor_id_04, speed_data_04, 5);
      }
      // 后退
      if (Straight_f < 0)
      {
        // 数据转换（1&3轮）
        // std::cout << "十进制数: " << V_x << std::endl;
        int V_x_1_3 = -round(V_x);
        // std::cout << "十进制数00: " << V_x_1_3 << std::endl;
        std::ostringstream stream_1_3;
        stream_1_3 << std::hex << V_x_1_3;
        std::string hexStr_1_3 = stream_1_3.str();
        // std::cout << "十六进制数: " << hexStr_1_3 << std::endl;
        // 左前轮速度（1）
        unsigned int num_1 = std::stoul(hexStr_1_3, nullptr, 16);
        output[0] = num_1;
        BYTE speed_data_01[5] = {
            0x1d,
            output[0] & 0xFF,         // 获取最低字节
            (output[0] >> 8) & 0xFF,  // 获取次低字节
            (output[0] >> 16) & 0xFF, // 获取次高字节
            (output[0] >> 24) & 0xFF  // 获取最高字节
        };
        // 左后轮速度（3）
        unsigned int num_3 = std::stoul(hexStr_1_3, nullptr, 16);
        output[1] = num_3;
        BYTE speed_data_03[5] = {
            0x1d,
            output[1] & 0xFF,         // 获取最低字节
            (output[1] >> 8) & 0xFF,  // 获取次低字节
            (output[1] >> 16) & 0xFF, // 获取次高字节
            (output[1] >> 24) & 0xFF  // 获取最高字节
        };
        // 数据转换（2&4轮）
        int V_x_2_4 = round(V_x);
        std::ostringstream stream_2_4;
        stream_2_4 << std::hex << V_x_2_4;
        std::string hexStr_2_4 = stream_2_4.str();
        // 右前轮速度(2)
        unsigned int num_2 = std::stoul(hexStr_2_4, nullptr, 16);
        output[1] = num_2;
        BYTE speed_data_02[5] = {
            0x1d,
            output[1] & 0xFF,         // 获取最低字节
            (output[1] >> 8) & 0xFF,  // 获取次低字节
            (output[1] >> 16) & 0xFF, // 获取次高字节
            (output[1] >> 24) & 0xFF  // 获取最高字节
        };
        // 右后轮速度(4)
        unsigned int num_4 = std::stoul(hexStr_2_4, nullptr, 16);
        output[1] = num_4;
        BYTE speed_data_04[5] = {
            0x1d,
            output[1] & 0xFF,         // 获取最低字节
            (output[1] >> 8) & 0xFF,  // 获取次低字节
            (output[1] >> 16) & 0xFF, // 获取次高字节
            (output[1] >> 24) & 0xFF  // 获取最高字节
        };

        // 发送数据
        SendData(send_motor_speed, motor_id_01, speed_data_01, 5);
        SendData(send_motor_speed, motor_id_02, speed_data_02, 5);
        SendData(send_motor_speed, motor_id_03, speed_data_03, 5);
        SendData(send_motor_speed, motor_id_04, speed_data_04, 5);
        turnSpeed = 0;
      }
      // 左转
      if (Turn_f > 0)
      {
        // // 转向时的直线速度
        // int V_x_t;
        // // 转向时的旋转速度
        // int V_x_f;

        int turnspeed;
        turnspeed = V_x;
        // 数据转换（1&3轮）
        int V_x_1_3 = -round(turnspeed);
        std::ostringstream stream_1_3;
        stream_1_3 << std::hex << V_x_1_3;
        std::string hexStr_1_3 = stream_1_3.str();
        // 左前轮速度（1）
        unsigned int num_1 = std::stoul(hexStr_1_3, nullptr, 16);
        output[0] = num_1;
        BYTE speed_data_01[5] = {
            0x1d,
            output[0] & 0xFF,         // 获取最低字节
            (output[0] >> 8) & 0xFF,  // 获取次低字节
            (output[0] >> 16) & 0xFF, // 获取次高字节
            (output[0] >> 24) & 0xFF  // 获取最高字节
        };
        // 左后轮速度（3）
        unsigned int num_3 = std::stoul(hexStr_1_3, nullptr, 16);
        output[2] = num_3;
        BYTE speed_data_03[5] = {
            0x1d,
            output[2] & 0xFF,         // 获取最低字节
            (output[2] >> 8) & 0xFF,  // 获取次低字节
            (output[2] >> 16) & 0xFF, // 获取次高字节
            (output[2] >> 24) & 0xFF  // 获取最高字节
        };
        // 数据转换（2&4轮）
        int V_x_2_4 = -round(turnspeed);
        std::ostringstream stream_2_4;
        stream_2_4 << std::hex << V_x_2_4;
        std::string hexStr_2_4 = stream_2_4.str();
        // 右前轮速度(2)
        unsigned int num_2 = std::stoul(hexStr_2_4, nullptr, 16);
        output[1] = num_2;
        BYTE speed_data_02[5] = {
            0x1d,
            output[1] & 0xFF,         // 获取最低字节
            (output[1] >> 8) & 0xFF,  // 获取次低字节
            (output[1] >> 16) & 0xFF, // 获取次高字节
            (output[1] >> 24) & 0xFF  // 获取最高字节
        };
        // 右后轮速度(4)
        unsigned int num_4 = std::stoul(hexStr_2_4, nullptr, 16);
        output[3] = num_4;
        BYTE speed_data_04[5] = {
            0x1d,
            output[3] & 0xFF,         // 获取最低字节
            (output[3] >> 8) & 0xFF,  // 获取次低字节
            (output[3] >> 16) & 0xFF, // 获取次高字节
            (output[3] >> 24) & 0xFF  // 获取最高字节
        };
        // 发送数据
        SendData(send_motor_speed, motor_id_01, speed_data_01, 5);
        SendData(send_motor_speed, motor_id_02, speed_data_02, 5);
        SendData(send_motor_speed, motor_id_03, speed_data_03, 5);
        SendData(send_motor_speed, motor_id_04, speed_data_04, 5);
      }
      // 右转
      if (Turn_f < 0)
      {
        // // 转向时的直线速度
        // int V_x_t = 200;
        // // 转向时的旋转速度
        // int V_x_f = 200;
        int turnspeed;
        turnspeed = V_x;
        // 数据转换（1&3轮）
        int V_x_1_3 = round(turnspeed);
        std::ostringstream stream_1_3;
        stream_1_3 << std::hex << V_x_1_3;
        std::string hexStr_1_3 = stream_1_3.str();
        // 左前轮速度（1）
        unsigned int num_1 = std::stoul(hexStr_1_3, nullptr, 16);
        output[0] = num_1;
        BYTE speed_data_01[5] = {
            0x1d,
            output[0] & 0xFF,         // 获取最低字节
            (output[0] >> 8) & 0xFF,  // 获取次低字节
            (output[0] >> 16) & 0xFF, // 获取次高字节
            (output[0] >> 24) & 0xFF  // 获取最高字节
        };
        // 左后轮速度（3）
        unsigned int num_3 = std::stoul(hexStr_1_3, nullptr, 16);
        output[2] = num_3;
        BYTE speed_data_03[5] = {
            0x1d,
            output[2] & 0xFF,         // 获取最低字节
            (output[2] >> 8) & 0xFF,  // 获取次低字节
            (output[2] >> 16) & 0xFF, // 获取次高字节
            (output[2] >> 24) & 0xFF  // 获取最高字节
        };
        // 数据转换（2&4轮）
        int V_x_2_4 = round(turnspeed);
        std::ostringstream stream_2_4;
        stream_2_4 << std::hex << V_x_2_4;
        std::string hexStr_2_4 = stream_2_4.str();
        // 右前轮速度(2)
        unsigned int num_2 = std::stoul(hexStr_2_4, nullptr, 16);
        output[1] = num_2;
        BYTE speed_data_02[5] = {
            0x1d,
            output[1] & 0xFF,         // 获取最低字节
            (output[1] >> 8) & 0xFF,  // 获取次低字节
            (output[1] >> 16) & 0xFF, // 获取次高字节
            (output[1] >> 24) & 0xFF  // 获取最高字节
        };
        // 右后轮速度(4)
        unsigned int num_4 = std::stoul(hexStr_2_4, nullptr, 16);
        output[3] = num_4;
        BYTE speed_data_04[5] = {
            0x1d,
            output[3] & 0xFF,         // 获取最低字节
            (output[3] >> 8) & 0xFF,  // 获取次低字节
            (output[3] >> 16) & 0xFF, // 获取次高字节
            (output[3] >> 24) & 0xFF  // 获取最高字节
        };
        // 发送数据
        SendData(send_motor_speed, motor_id_01, speed_data_01, 5);
        SendData(send_motor_speed, motor_id_02, speed_data_02, 5);
        SendData(send_motor_speed, motor_id_03, speed_data_03, 5);
        SendData(send_motor_speed, motor_id_04, speed_data_04, 5);
      }

      // 等待毫秒 df:20
      std::this_thread::sleep_for(std::chrono::milliseconds(20));
      SendData(send_motor_speed, motor_id_01, speed_zero, 5);
      SendData(send_motor_speed, motor_id_02, speed_zero, 5);
      SendData(send_motor_speed, motor_id_03, speed_zero, 5);
      SendData(send_motor_speed, motor_id_04, speed_zero, 5);
    }
    // if (flag == 1)
    // {
    //   std::cout << "in:" << pitch << std::endl;
    //   pitch = pitch / 180 * 3.1415;
    //   // std::cout << "in:" << pitch << std::endl;
    //   double sin_val = std::sin(pitch);
    //   double cos_val = std::cos(pitch);
    //   V_y = V_x * cos_val / sin_val;
    //   std::cout << V_y << std::endl;
    //   // 前进
    //   if (Straight_f > 0)
    //   {
    //     std::cout << "补偿前进" << std::endl;
    //     // 左前轮速度(1)
    //     int V_y_1 = round(V_y);
    //     std::ostringstream stream_1;
    //     stream_1 << std::hex << V_y_1;
    //     std::string hexStr_1 = stream_1.str();
    //     unsigned int num_1 = std::stoul(hexStr_1, nullptr, 16);
    //     output[0] = num_1;
    //     BYTE speed_data_01[5] = {
    //         0x1d,
    //         output[0] & 0xFF,         // 获取最低字节
    //         (output[0] >> 8) & 0xFF,  // 获取次低字节
    //         (output[0] >> 16) & 0xFF, // 获取次高字节
    //         (output[0] >> 24) & 0xFF  // 获取最高字节
    //     };
    //     // 右前轮速度（2）
    //     int V_y_2 = -round(V_y);
    //     std::ostringstream stream_2;
    //     stream_2 << std::hex << V_y_2;
    //     std::string hexStr_2 = stream_2.str();
    //     unsigned int num_2 = std::stoul(hexStr_2, nullptr, 16);
    //     output[0] = num_2;
    //     BYTE speed_data_02[5] = {
    //         0x1d,
    //         output[0] & 0xFF,         // 获取最低字节
    //         (output[0] >> 8) & 0xFF,  // 获取次低字节
    //         (output[0] >> 16) & 0xFF, // 获取次高字节
    //         (output[0] >> 24) & 0xFF  // 获取最高字节
    //     };
    //     // 左后轮速度（3）
    //     int V_x_3 = round(V_x);
    //     std::ostringstream stream_3;
    //     stream_3 << std::hex << V_x_3;
    //     std::string hexStr_3 = stream_3.str();
    //     unsigned int num_3 = std::stoul(hexStr_3, nullptr, 16);
    //     output[0] = num_3;
    //     BYTE speed_data_03[5] = {
    //         0x1d,
    //         output[0] & 0xFF,         // 获取最低字节
    //         (output[0] >> 8) & 0xFF,  // 获取次低字节
    //         (output[0] >> 16) & 0xFF, // 获取次高字节
    //         (output[0] >> 24) & 0xFF  // 获取最高字节
    //     };
    //     // 右后轮速度（4）
    //     int V_x_4 = -round(V_x);
    //     std::ostringstream stream_4;
    //     stream_4 << std::hex << V_x_4;
    //     std::string hexStr_4 = stream_4.str();
    //     unsigned int num_4 = std::stoul(hexStr_4, nullptr, 16);
    //     output[0] = num_4;
    //     BYTE speed_data_04[5] = {
    //         0x1d,
    //         output[0] & 0xFF,         // 获取最低字节
    //         (output[0] >> 8) & 0xFF,  // 获取次低字节
    //         (output[0] >> 16) & 0xFF, // 获取次高字节
    //         (output[0] >> 24) & 0xFF  // 获取最高字节
    //     };

    //     // 发送数据
    //     SendData(send_motor_speed, motor_id_01, speed_data_01, 5);
    //     SendData(send_motor_speed, motor_id_02, speed_data_02, 5);
    //     SendData(send_motor_speed, motor_id_03, speed_data_03, 5);
    //     SendData(send_motor_speed, motor_id_04, speed_data_04, 5);
    //   }
    //   // 后退
    //   if (Straight_f < 0)
    //   {
    //     std::cout << "补偿后退" << std::endl;
    //     // 左前轮速度（1）
    //     int V_y_1 = -round(V_y);
    //     std::ostringstream stream_1;
    //     stream_1 << std::hex << V_y_1;
    //     std::string hexStr_1 = stream_1.str();
    //     unsigned int num_1 = std::stoul(hexStr_1, nullptr, 16);
    //     output[0] = num_1;
    //     BYTE speed_data_01[5] = {
    //         0x1d,
    //         output[0] & 0xFF,         // 获取最低字节
    //         (output[0] >> 8) & 0xFF,  // 获取次低字节
    //         (output[0] >> 16) & 0xFF, // 获取次高字节
    //         (output[0] >> 24) & 0xFF  // 获取最高字节
    //     };
    //     // 右前轮速度(2)
    //     int V_y_2 = round(V_y);
    //     std::ostringstream stream_2;
    //     stream_2 << std::hex << V_y_2;
    //     std::string hexStr_2 = stream_2.str();
    //     unsigned int num_2 = std::stoul(hexStr_2, nullptr, 16);
    //     output[1] = num_2;
    //     BYTE speed_data_02[5] = {
    //         0x1d,
    //         output[1] & 0xFF,         // 获取最低字节
    //         (output[1] >> 8) & 0xFF,  // 获取次低字节
    //         (output[1] >> 16) & 0xFF, // 获取次高字节
    //         (output[1] >> 24) & 0xFF  // 获取最高字节
    //     };
    //     // 左后轮速度（3）
    //     int V_x_3 = -round(V_x);
    //     std::ostringstream stream_3;
    //     stream_3 << std::hex << V_x_3;
    //     std::string hexStr_3 = stream_3.str();
    //     unsigned int num_3 = std::stoul(hexStr_3, nullptr, 16);
    //     output[1] = num_3;
    //     BYTE speed_data_03[5] = {
    //         0x1d,
    //         output[1] & 0xFF,         // 获取最低字节
    //         (output[1] >> 8) & 0xFF,  // 获取次低字节
    //         (output[1] >> 16) & 0xFF, // 获取次高字节
    //         (output[1] >> 24) & 0xFF  // 获取最高字节
    //     };

    //     // 右后轮速度(4)
    //     int V_x_4 = round(V_x);
    //     std::ostringstream stream_4;
    //     stream_4 << std::hex << V_x_4;
    //     std::string hexStr_4 = stream_4.str();
    //     unsigned int num_4 = std::stoul(hexStr_4, nullptr, 16);
    //     output[1] = num_4;
    //     BYTE speed_data_04[5] = {
    //         0x1d,
    //         output[1] & 0xFF,         // 获取最低字节
    //         (output[1] >> 8) & 0xFF,  // 获取次低字节
    //         (output[1] >> 16) & 0xFF, // 获取次高字节
    //         (output[1] >> 24) & 0xFF  // 获取最高字节
    //     };

    //     // 发送数据
    //     SendData(send_motor_speed, motor_id_01, speed_data_01, 5);
    //     SendData(send_motor_speed, motor_id_02, speed_data_02, 5);
    //     SendData(send_motor_speed, motor_id_03, speed_data_03, 5);
    //     SendData(send_motor_speed, motor_id_04, speed_data_04, 5);
    //   }
    //   // 等待毫秒
    //   std::this_thread::sleep_for(std::chrono::milliseconds(20));
    //   SendData(send_motor_speed, motor_id_01, speed_zero, 5);
    //   SendData(send_motor_speed, motor_id_02, speed_zero, 5);
    //   SendData(send_motor_speed, motor_id_03, speed_zero, 5);
    //   SendData(send_motor_speed, motor_id_04, speed_zero, 5);
    // }

    // if (flag == 2)
    // {
    //   while (true)
    //   // 左高右低 左转
    //   {
    //     // 防止误操作
    //     if (fabs(roll) > 45)
    //     {
    //       break;
    //     }
    //     if (roll > 0)
    //     {
    //       // 转向时的直线速度
    //       int V_x_t = 50;
    //       // 转向时的旋转速度
    //       int V_x_f = 50;
    //       // 数据转换（1&3轮）
    //       int V_x_1_3 = -round(V_x_t + V_x_f);
    //       std::ostringstream stream_1_3;
    //       stream_1_3 << std::hex << V_x_1_3;
    //       std::string hexStr_1_3 = stream_1_3.str();
    //       // 左前轮速度（1）
    //       unsigned int num_1 = std::stoul(hexStr_1_3, nullptr, 16);
    //       output[0] = num_1;
    //       BYTE speed_data_01[5] = {
    //           0x1d,
    //           output[0] & 0xFF,         // 获取最低字节
    //           (output[0] >> 8) & 0xFF,  // 获取次低字节
    //           (output[0] >> 16) & 0xFF, // 获取次高字节
    //           (output[0] >> 24) & 0xFF  // 获取最高字节
    //       };
    //       // 左后轮速度（3）
    //       unsigned int num_3 = std::stoul(hexStr_1_3, nullptr, 16);
    //       output[2] = num_3;
    //       BYTE speed_data_03[5] = {
    //           0x1d,
    //           output[2] & 0xFF,         // 获取最低字节
    //           (output[2] >> 8) & 0xFF,  // 获取次低字节
    //           (output[2] >> 16) & 0xFF, // 获取次高字节
    //           (output[2] >> 24) & 0xFF  // 获取最高字节
    //       };
    //       // 数据转换（2&4轮）
    //       int V_x_2_4 = -round(V_x_t + V_x_f);
    //       std::ostringstream stream_2_4;
    //       stream_2_4 << std::hex << V_x_2_4;
    //       std::string hexStr_2_4 = stream_2_4.str();
    //       // 右前轮速度(2)
    //       unsigned int num_2 = std::stoul(hexStr_2_4, nullptr, 16);
    //       output[1] = num_2;
    //       BYTE speed_data_02[5] = {
    //           0x1d,
    //           output[1] & 0xFF,         // 获取最低字节
    //           (output[1] >> 8) & 0xFF,  // 获取次低字节
    //           (output[1] >> 16) & 0xFF, // 获取次高字节
    //           (output[1] >> 24) & 0xFF  // 获取最高字节
    //       };
    //       // 右后轮速度(4)
    //       unsigned int num_4 = std::stoul(hexStr_2_4, nullptr, 16);
    //       output[3] = num_4;
    //       BYTE speed_data_04[5] = {
    //           0x1d,
    //           output[3] & 0xFF,         // 获取最低字节
    //           (output[3] >> 8) & 0xFF,  // 获取次低字节
    //           (output[3] >> 16) & 0xFF, // 获取次高字节
    //           (output[3] >> 24) & 0xFF  // 获取最高字节
    //       };
    //       // 发送数据
    //       SendData(send_motor_speed, motor_id_01, speed_data_01, 5);
    //       SendData(send_motor_speed, motor_id_02, speed_data_02, 5);
    //       SendData(send_motor_speed, motor_id_03, speed_data_03, 5);
    //       SendData(send_motor_speed, motor_id_04, speed_data_04, 5);
    //       std::this_thread::sleep_for(std::chrono::milliseconds(20));
    //     }
    //     // 左低右高 右转
    //     if (roll < 0)
    //     {
    //       // 转向时的直线速度
    //       int V_x_t = 50;
    //       // 转向时的旋转速度
    //       int V_x_f = 50;
    //       // 数据转换（1&3轮）
    //       int V_x_1_3 = round(V_x_t + V_x_f);
    //       std::ostringstream stream_1_3;
    //       stream_1_3 << std::hex << V_x_1_3;
    //       std::string hexStr_1_3 = stream_1_3.str();
    //       // 左前轮速度（1）
    //       unsigned int num_1 = std::stoul(hexStr_1_3, nullptr, 16);
    //       output[0] = num_1;
    //       BYTE speed_data_01[5] = {
    //           0x1d,
    //           output[0] & 0xFF,         // 获取最低字节
    //           (output[0] >> 8) & 0xFF,  // 获取次低字节
    //           (output[0] >> 16) & 0xFF, // 获取次高字节
    //           (output[0] >> 24) & 0xFF  // 获取最高字节
    //       };
    //       // 左后轮速度（3）
    //       unsigned int num_3 = std::stoul(hexStr_1_3, nullptr, 16);
    //       output[2] = num_3;
    //       BYTE speed_data_03[5] = {
    //           0x1d,
    //           output[2] & 0xFF,         // 获取最低字节
    //           (output[2] >> 8) & 0xFF,  // 获取次低字节
    //           (output[2] >> 16) & 0xFF, // 获取次高字节
    //           (output[2] >> 24) & 0xFF  // 获取最高字节
    //       };
    //       // 数据转换（2&4轮）
    //       int V_x_2_4 = round(V_x_t + V_x_f);
    //       std::ostringstream stream_2_4;
    //       stream_2_4 << std::hex << V_x_2_4;
    //       std::string hexStr_2_4 = stream_2_4.str();
    //       // 右前轮速度(2)
    //       unsigned int num_2 = std::stoul(hexStr_2_4, nullptr, 16);
    //       output[1] = num_2;
    //       BYTE speed_data_02[5] = {
    //           0x1d,
    //           output[1] & 0xFF,         // 获取最低字节
    //           (output[1] >> 8) & 0xFF,  // 获取次低字节
    //           (output[1] >> 16) & 0xFF, // 获取次高字节
    //           (output[1] >> 24) & 0xFF  // 获取最高字节
    //       };
    //       // 右后轮速度(4)
    //       unsigned int num_4 = std::stoul(hexStr_2_4, nullptr, 16);
    //       output[3] = num_4;
    //       BYTE speed_data_04[5] = {
    //           0x1d,
    //           output[3] & 0xFF,         // 获取最低字节
    //           (output[3] >> 8) & 0xFF,  // 获取次低字节
    //           (output[3] >> 16) & 0xFF, // 获取次高字节
    //           (output[3] >> 24) & 0xFF  // 获取最高字节
    //       };
    //       // 发送数据
    //       SendData(send_motor_speed, motor_id_01, speed_data_01, 5);
    //       SendData(send_motor_speed, motor_id_02, speed_data_02, 5);
    //       SendData(send_motor_speed, motor_id_03, speed_data_03, 5);
    //       SendData(send_motor_speed, motor_id_04, speed_data_04, 5);
    //       std::this_thread::sleep_for(std::chrono::milliseconds(20));
    //     }
    //     if (fabs(roll) < 0.1)
    //     {
    //       // 停止电机
    //       SendData(send_motor_speed, motor_id_01, speed_zero, 5);
    //       SendData(send_motor_speed, motor_id_02, speed_zero, 5);
    //       SendData(send_motor_speed, motor_id_03, speed_zero, 5);
    //       SendData(send_motor_speed, motor_id_04, speed_zero, 5);
    //       std::this_thread::sleep_for(std::chrono::milliseconds(20));
    //       break;
    //     }
    //   }
    // }
  }

  // // 速度反馈函数
  // void on_rtsp_timer()
  // {
  //   rtspeed1_pub_->publish(rt1_data);
  //   rtspeed2_pub_->publish(rt2_data);
  //   rtspeed3_pub_->publish(rt3_data);
  //   rtspeed4_pub_->publish(rt4_data);
  //   /*
  //     问题详述：
  //     如果将查询速度，并且返回速度值，并发布的任务放在这里，会使得键盘控制出现卡顿，因此在下方多线程中实现该功能
  //     此处仅实现数据发布
  //     此处比较冗杂，可以用数组
  //   */
  // }
};

int main(int argc, char const *argv[])
{

  output[0] = 0;
  output[1] = 0;
  output[2] = 0;
  output[3] = 0;

  // 2.初始化ROS2客户端；
  rclcpp::init(argc, argv);
  // 初始化CAN设备
  Init_Can();
  // 4.调用spain函数，并传入节点对象指针；
  rclcpp::spin(std::make_shared<Can_Driver>());
  // 5.资源释放
  rclcpp::shutdown();
  // 停止电机
  SendData(send_motor_speed, motor_id_01, speed_zero, 5);
  SendData(send_motor_speed, motor_id_02, speed_zero, 5);
  SendData(send_motor_speed, motor_id_03, speed_zero, 5);
  SendData(send_motor_speed, motor_id_04, speed_zero, 5);
  // 速度降低后关闭电机
  SendData(send_motor_speed, motor_id_01, motor_stop, 1);
  SendData(send_motor_speed, motor_id_02, motor_stop, 1);
  SendData(send_motor_speed, motor_id_03, motor_stop, 1);
  SendData(send_motor_speed, motor_id_04, motor_stop, 1);
  // 关闭can设备
  VCI_CloseDevice(device_type, device_index);
  return 0;
}

// CAN设备初始化函数
void Init_Can(void)
{
  // int m_run0 = 1;
  // int ret;
  // 初始化CAN设备
  device_type = VCI_USBCAN2; // 设备类型修改
  device_index = 0;          // 设备的索引
  can_index = 0;             // CAN控制器的索引 CAN1为0，CAN2为1

  if (VCI_OpenDevice(device_type, device_index, 0) != STATUS_OK)
  {
    std::cout << "无法开启CAN设备" << std::endl;
    return;
  }
  else
  {
    std::cout << "设备开启,如需退出请按“CTRL + C”" << std::endl;
  }

  if (VCI_ReadBoardInfo(device_type, device_index, &board_info) != STATUS_OK)
  {
    std::cout << "获取设备信息失败" << std::endl;
    VCI_CloseDevice(device_type, device_index);
    return;
  }
  // 设置CAN初始化配置
  init_config.AccCode = 0X00000000;
  init_config.AccMask = 0xFFFFFFFF;
  init_config.Filter = 2; // 只接受标准帧
  init_config.Mode = 0;
  init_config.Timing0 = 0x00;
  init_config.Timing1 = 0x14; // 波特率：1000kps

  if (VCI_InitCAN(device_type, device_index, can_index, &init_config) != STATUS_OK)
  {
    std::cout << "初始化CAN参数失败" << std::endl;
    VCI_CloseDevice(device_type, device_index);
    return;
  }

  // 开始CAN通讯
  if (VCI_StartCAN(device_type, device_index, can_index) != STATUS_OK)
  {
    std::cout << "开启CAN通道失败" << std::endl;
    VCI_CloseDevice(device_type, device_index);
    return;
  }

  // ret = pthread_create(&threadid, NULL, receive_func, &m_run0);
}

// 指令发送函数
void SendData(VCI_CAN_OBJ &handle_obj, const int id, const BYTE *data, int dataLen)
{
  handle_obj.ID = id;
  handle_obj.RemoteFlag = 0;
  handle_obj.ExternFlag = 0;
  handle_obj.DataLen = dataLen;
  for (int i = 0; i < handle_obj.DataLen; i++)
  {
    handle_obj.Data[i] = data[i];
  }
  if (VCI_Transmit(device_type, device_index, can_index, &handle_obj, 1) > 0)
  {
  }
  else
  {
    std::cout << "初始化错误！" << std::endl;
  }
}

int pos_val(float pos_ang)
{
  int pos_ang_int = round(pos_ang / 360 * 101 * 65536);
  return pos_ang_int;
}
// void *receive_func(void *param)
// {
//   int reclen;
//   uint8_t receiveData[5];
//   uint8_t realData[2];
//   VCI_CAN_OBJ rec[2]; // 接收长度
//   (void)param;

//   while (rclcpp::ok())
//   {
//     // std::cout <<"ok"<< std::endl;
//     SendData(get_motor_receive, motor_id_01, get_speed, 1);
//     std::this_thread::sleep_for(std::chrono::milliseconds(300));
//     if (reclen = VCI_Receive(device_type, device_index, can_index, rec, Len, WaitTime) > 0)
//     {
//       for (int j = 0; j < reclen; j++)
//       {
//         switch (rec[j].ID)
//         {
//         case 0x001:
//         {
//           // int id = rec[j].ID;
//           // std::cout << "当前ID是:" << id << std::endl;
//           for (int i = 0; i < 5; i++)
//           {
//             // printf("%02x", rec[j].Data[i]);
//             receiveData[i] = rec[j].Data[i];
//           }
//           realData[0] = receiveData[2];
//           realData[1] = receiveData[1];
//           int16_t hexval = (static_cast<uint16_t>(realData[0] << 8) | realData[1]);
//           int decval = static_cast<int>(hexval);
//           // printf("%d", decval);
//           // 发布信息
//           rt1_data.data = decval;
//           std::this_thread::sleep_for(std::chrono::milliseconds(10));
//           SendData(get_motor_receive, motor_id_02, get_speed, 1);
//           break;
//         }

//         case 0x002:
//         {
//           // int id = rec[j].ID;
//           // std::cout << "当前ID是:" << id << std::endl;
//           for (int i = 0; i < 5; i++)
//           {
//             receiveData[i] = rec[j].Data[i];
//           }
//           realData[0] = receiveData[2];
//           realData[1] = receiveData[1];
//           int16_t hexval = (static_cast<uint16_t>(realData[0] << 8) | realData[1]);
//           int decval = static_cast<int>(hexval);
//           // 发布信息
//           rt2_data.data = decval;
//           // std::cout << decval << std::endl;
//           std::this_thread::sleep_for(std::chrono::milliseconds(10));
//           SendData(get_motor_receive, motor_id_03, get_speed, 1);
//           break;
//         }

//         case 0x003:
//         {
//           // int id = rec[j].ID;
//           // std::cout << "当前ID是:" << id << std::endl;
//           for (int i = 0; i < 5; i++)
//           {
//             receiveData[i] = rec[j].Data[i];
//           }
//           realData[0] = receiveData[2];
//           realData[1] = receiveData[1];
//           int16_t hexval = (static_cast<uint16_t>(realData[0] << 8) | realData[1]);
//           int decval = static_cast<int>(hexval);
//           // 发布信息
//           rt3_data.data = decval;
//           // std::cout << decval << std::endl;
//           std::this_thread::sleep_for(std::chrono::milliseconds(10));
//           SendData(get_motor_receive, motor_id_04, get_speed, 1);
//           break;
//         }

//         case 0x004:
//         {
//           // int id = rec[j].ID;
//           // std::cout << "当前ID是:" << id << std::endl;
//           for (int i = 0; i < 5; i++)
//           {
//             receiveData[i] = rec[j].Data[i];
//           }
//           realData[0] = receiveData[2];
//           realData[1] = receiveData[1];
//           int16_t hexval = (static_cast<uint16_t>(realData[0] << 8) | realData[1]);
//           int decval = static_cast<int>(hexval);
//           // 发布信息
//           rt4_data.data = decval;
//           // std::cout << decval << std::endl;
//           break;
//         }
//         }
//       }
//     }
//   }

//   pthread_exit(0);
// }
